#ifndef SBUS_HPP
#define SBUS_HPP
#include <Arduino.h>

class SBUS {
  public:
	SBUS(uint8_t pin, HardwareSerial *serial) {
		_pin = pin;
		_serial = serial;
	};
	SBUS(uint8_t pin) {
		_pin = pin;
		_serial = &Serial2;
	};
	SBUS() {
		_serial = &Serial2;
		_pin = 6;
	};
	void setup() {
		pinMode(_pin, INPUT);
		_serial->begin(100000, SERIAL_8E2, _pin, 38);
		memset(raw_data, 0, sizeof(raw_data));
		memset(channel_data, 0, sizeof(channel_data));
		flag = 0;
		xTaskCreate(loop_task, "loop_task", 4096, this, 2, NULL);
	};
	static void loop_task(void *p) {
		SBUS *obj = (SBUS *)p;
		while (true) {
			if (obj->_serial->available() < 25) {
				delay(1);
				continue;
			}

			while (obj->_serial->peek() != 0xf) {
				obj->_serial->read();
				delay(1);
			}
			while (obj->_serial->available() < 25) {
				delay(1);
			}
			obj->_serial->readBytes(obj->raw_data, 25);
			if (obj->raw_data[0] != 0x0f) {
				continue;
			}
			int check = ((int16_t)obj->raw_data[1] >> 0 |
			                ((int16_t)obj->raw_data[2] << 8)) &
			            0x07FF;
			uint16_t channel_data_check[16];

			//每11位为一个通道
			channel_data_check[0] = ((int16_t)obj->raw_data[1] >> 0 |
			                            ((int16_t)obj->raw_data[2] << 8)) &
			                        0x07FF;
			channel_data_check[1] = ((int16_t)obj->raw_data[2] >> 3 |
			                            ((int16_t)obj->raw_data[3] << 5)) &
			                        0x07FF;
			channel_data_check[2] = ((int16_t)obj->raw_data[3] >> 6 |
			                            ((int16_t)obj->raw_data[4] << 2) |
			                            (int16_t)obj->raw_data[5] << 10) &
			                        0x07FF;
			channel_data_check[3] = ((int16_t)obj->raw_data[5] >> 1 |
			                            ((int16_t)obj->raw_data[6] << 7)) &
			                        0x07FF;
			channel_data_check[4] = ((int16_t)obj->raw_data[6] >> 4 |
			                            ((int16_t)obj->raw_data[7] << 4)) &
			                        0x07FF;
			channel_data_check[5] = ((int16_t)obj->raw_data[7] >> 7 |
			                            ((int16_t)obj->raw_data[8] << 1) |
			                            (int16_t)obj->raw_data[9] << 9) &
			                        0x07FF;
			channel_data_check[6] = ((int16_t)obj->raw_data[9] >> 2 |
			                            ((int16_t)obj->raw_data[10] << 6)) &
			                        0x07FF;
			channel_data_check[7] = ((int16_t)obj->raw_data[10] >> 5 |
			                            ((int16_t)obj->raw_data[11] << 3)) &
			                        0x07FF;
			channel_data_check[8] = ((int16_t)obj->raw_data[12] >> 0 |
			                            ((int16_t)obj->raw_data[13] << 8)) &
			                        0x07FF;
			channel_data_check[9] = ((int16_t)obj->raw_data[13] >> 3 |
			                            ((int16_t)obj->raw_data[14] << 5)) &
			                        0x07FF;
			channel_data_check[10] = ((int16_t)obj->raw_data[14] >> 6 |
			                             ((int16_t)obj->raw_data[15] << 2) |
			                             (int16_t)obj->raw_data[16] << 10) &
			                         0x07FF;
			channel_data_check[11] = ((int16_t)obj->raw_data[16] >> 1 |
			                             ((int16_t)obj->raw_data[17] << 7)) &
			                         0x07FF;
			channel_data_check[12] = ((int16_t)obj->raw_data[17] >> 4 |
			                             ((int16_t)obj->raw_data[18] << 4)) &
			                         0x07FF;
			channel_data_check[13] = ((int16_t)obj->raw_data[18] >> 7 |
			                             ((int16_t)obj->raw_data[19] << 1) |
			                             (int16_t)obj->raw_data[20] << 9) &
			                         0x07FF;
			channel_data_check[14] = ((int16_t)obj->raw_data[20] >> 2 |
			                             ((int16_t)obj->raw_data[21] << 6)) &
			                         0x07FF;
			channel_data_check[15] = ((int16_t)obj->raw_data[21] >> 5 |
			                             ((int16_t)obj->raw_data[22] << 3)) &
			                         0x07FF;

			bool is_ok = true;
			for (int i = 0; i < 16; i++) {
				if (channel_data_check[i] > 1700 ||
				    channel_data_check[i] < 350) {
					is_ok = false;
				}
			}

			if (!is_ok) {
				continue;
			}

			memcpy(obj->channel_data, channel_data_check, 16 * 2);
			obj->flag = obj->raw_data[24];
			if (obj->is_first) {
				obj->is_first = false;
			}
		}
	}
	uint16_t operator[](uint8_t i) { return channel_data[i - 1]; }
	uint16_t get_channel_data(uint8_t i) { return channel_data[i - 1]; }
	uint8_t get_flag() { return flag; }
	bool is_first = true;

  protected:
	uint8_t _pin;
	HardwareSerial *_serial;
	uint8_t raw_data[25];
	uint16_t channel_data[16];
	uint8_t flag;
};

#endif
/*
                                              .=%@#=.                                               
                                            -*@@@@@@@#=.                                            
                                         .+%@@@@@@@@@@@@#=                                          
                                       -#@@@@@@@* =@@@@@@@@*:                                       
                                     =%@@@@@@@@=   -@@@@@@@@@#-                                     
                                  .+@@@@@@@@@@-     .@@@@@@@@@@%=                                   
                                .+@@@@@@@@@@@@-     +@@@@@@@@@@@@@+.                                
                               +@@@@@@@@@@@@@@@    .@@@@@@@@@@@@@@@@+.                              
                             =@@@@@@@@@@@@@@@%-     =%@@%@@@@@@@@@@@@@=                             
                           -%@@@@@@@@@@@@+..     .       -@@@@@@@@@@@@@%-                           
                         .#@@@@@@@@@@@@@#       -@+       +@@@@@@@@@@@@@@#:                         
                        +@@@@@@@@@@@@@@@@+     +@@@+     =@@@@@@@@@@@@@@@@@+                        
                      :%@@@@@@@@@@@@@@@@@+    *@@@@*     =@@@@@@@@@@@@@@@@@@%-                      
                     +@@@@@@@@@@@@@@#+*+-   .#@@@@+       :+*+*@@@@@@@@@@@@@@@*                     
                   :%@@@@@@@@@@@@@@+       :%@@@@-    .-       -@@@@@@@@@@@@@@@%:                   
                  =@@@@@@@@@@@@@@@@-      -@@@@%:    .%@+      =@@@@@@@@@@@@@@@@@=                  
                 *@@@@@@@@@@@@@@@@@@.    =@@@@#.    -@@@@+    =@@@@@@@@@@@@@@@@@@@#                 
               .%@@@@@@@@@@@@@@@@@@+    +@@@@*     =@@@@%:    .#@@@@@@@@@@@@@@@@@@@%.               
              :@@@@@@@@@@@@@@@%:::.    #@@@@+     +@@@@#        .::.*@@@@@@@@@@@@@@@@-              
             -@@@@@@@@@@@@@@@%       .%@@@@=     *@@@@*     +-       *@@@@@@@@@@@@@@@@=             
            =@@@@@@@@@@@@@@@@@#.    -@@@@@-    :%@@@@=    .#@@+     +@@@@@@@@@@@@@@@@@@=            
           =@@@@@@@@@@@@@@@@@@@:    =====.     -+===:     :====     @@@@@@@@@@@@@@@@@@@@+           
          +@@@@@@@@@@@@@@@#%%#-                                     :*%%#%@@@@@@@@@@@@@@@+          
         =@@@@@@@@@@@@@@%.       ...........................              *@@@@@@@@@@@@@@@=         
        =@@@@@@@@@@@@@@@+      .#@@@@@@@@@@@@@@@@@@@@@@@@@@#     .*:      =@@@@@@@@@@@@@@@@-        
       -@@@@@@@@@@@@@@@@@=    .%@@@@@@@@@@@@@@@@@@@@@@@@@@#     :@@@-    =@@@@@@@@@@@@@@@@@@:       
      :@@@@@@@@@@@@@@@@@%.   -@@@@%+=====================:     -@@@@%    :%@@@@@@@@@@@@@@@@@@.      
      %@@@@@@@@@@@@@=-=:    =@@@@#.                           +@@@@#.      -=--%@@@@@@@@@@@@@%      
     #@@@@@@@@@@@@@:       +@@@@*      ............. .       *@@@@*             %@@@@@@@@@@@@@+     
    =@@@@@@@@@@@@@@#.     #@@@@+     +@@@@@@@@@@@@@@@#.    .#@@@@+     +#.     +@@@@@@@@@@@@@@@:    
   .@@@@@@@@@@@@@@@@-   .%@@@@=     *@@@@@@@@@@@@@@@#     :%@@@@-     *@@%:    @@@@@@@@@@@@@@@@%    
   %@@@@@@@@@@@%%%#=   :@@@@@:    .#@@@@+-----------     -@@@@@:     #@@@@=    :#%%%@@@@@@@@@@@@*   
  =@@@@@@@@@@@=       -@@@@%.    :%@@@@-                =@@@@%.    .%@@@@=          :%@@@@@@@@@@@:  
  @@@@@@@@@@@%.      =@@@@#     -@@@@%:    .:::-:      +@@@@#     :@@@@@:    .       +@@@@@@@@@@@#  
 +@@@@@@@@@@@@@.    *@@@@*     =@@@@#.    -@@@@@:     #@@@@+     =@@@@%.    -@#     +@@@@@@@@@@@@@- 
.@@@@@@@@@@@@@#    *@%@%=     +@@@@*     =@@@@#.    .#@@@%=     +@@@@#     =@@@%.   =@@@@@@@@@@@@@% 
+@@@@@@@@*-==-                .          .           . ..       .....      .....     .=+=+@@@@@@@@@-
%@@@@@@@+                                                                                 -@@@@@@@@#
@@@@@@@-       =#%#=     -#%%#-     -#%%*.     +%%%*.    .*%%#=     :#%%#-     =%%%*.      .#@@@@@@@
@@@@@@=.::::::*@@@@@*:::-@@@@@@-:::=@@@@@%::::*@@@@@#::::%@@@@@+:---@@@@@@=---+@@@@@%------:=@@@@@@@
=@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@+
 *@@@@@@@@@@@@@@@@@@@@@@@@@@@%%##**++===----:::::------===++***##%%@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@* 
  -#@@@@@@@@@@@@@@@@%#*+=-:.                                        ..::-=+*##%@@@@@@@@@@@@@@@@@#-  
    :=*%@@@@@%#*=-:                                                             .:-=+*#%%%%##+-.    
                                                                                        
        K####      #####     ###    ###  ######.   ##########     K##    ### ###    ##W    ####W    
       #######    #######    ###    ###  ########  ##########     ###    ### ###   ###   W######    
      W###G####  ###W ####   ###    ###  ######### ##########     ###    ###  ##   ###   ###W####   
      ###   ###  ###   ###   ###    ##  ###    ###    ###         ###    ###  ### t##   ###   ###   
     G##    #   ###    ###   ##     ##  ###    ###    ###         ###    ###  ### ###   ##W         
     ###        ###    ###   ##    ###  ###    ###    ###         ##L    ##   ### ##   ###          
     ###        ###    ###  K##    ###  ###    ###    ###         ##     ##    #####   ###          
     ###       ,##     ###  ###    ###  ###   ###,    ##         G##    ###    ####    ###          
    W##        ###     ###  ###    ###  #########     ##         ##########    ####    ###          
    ###        ###     ###  ###    ###  ########     ###         ##########    ###i   K##           
    ###        ###     ###  ###    ##  #######       ###         ###    ###    ####   ###           
    ###        ###     ###  ##     ##  ###           ###         ###    ###   ##W##   ###           
    ###        ###     ##i  ##    ###  ###           ###         ###    ##    ## ##   ###           
    ###        ###    ###  ,##    ###  ###           ###         ##     ##   ### ##   ###           
    ###    ### ###    ###  K##    ###  ###           ##         t##    ###   ##  ###  ###    ###    
    ###   G##i ###   ###   .##   ###.  ##t           ##         ###    ###  ###  ###  W##,   ###    
     ########  W##W#####    ########   ##           ###         ###    ###  ##    ##   ####W###     
     #######    #######     #######   ###           ###         ###    ### ###    ##.  #######      
      #####      #####       #####    ###           ###         ###    ### ##W    ###   #####       
                   ###                                                                              
                   ###                                                                              
                   #####                                                                            
                    ####                                                                            
                      K                                                                             
*/